home *** CD-ROM | disk | FTP | other *** search
/ SGI Developer Toolbox 6.1 / SGI Developer Toolbox 6.1 - Disc 4.iso / src / haeberli / libgutil / homo2.c < prev    next >
C/C++ Source or Header  |  1994-08-01  |  5KB  |  208 lines
  1. /*
  2.  * Copyright 1991, 1992, 1993, 1994, Silicon Graphics, Inc.
  3.  * All Rights Reserved.
  4.  *
  5.  * This is UNPUBLISHED PROPRIETARY SOURCE CODE of Silicon Graphics, Inc.;
  6.  * the contents of this file may not be disclosed to third parties, copied or
  7.  * duplicated in any form, in whole or in part, without the prior written
  8.  * permission of Silicon Graphics, Inc.
  9.  *
  10.  * RESTRICTED RIGHTS LEGEND:
  11.  * Use, duplication or disclosure by the Government is subject to restrictions
  12.  * as set forth in subdivision (c)(1)(ii) of the Rights in Technical Data
  13.  * and Computer Software clause at DFARS 252.227-7013, and/or in similar or
  14.  * successor clauses in the FAR, DOD or NASA FAR Supplement. Unpublished -
  15.  * rights reserved under the Copyright Laws of the United States.
  16.  */
  17. /*
  18.  *    homo2 -
  19.  *        Homogeneous mapping in 2 dimensions. 
  20.  *
  21.  *                Tom Davis - 1986
  22.  */
  23. #include "stdio.h"
  24.  
  25. float det3();
  26.  
  27. maptomat3(src,dst,mat)
  28. float src[4][3], dst[4][3];
  29. float mat[3][3];
  30. {
  31.     float srctrans[3][3], dsttrans[3][3];
  32.     float inv[3][3];
  33.  
  34.     if ((det3(src, 0, 1, 2) == 0.0) ||    
  35.             (det3(src, 0, 1, 3) == 0.0) ||    
  36.             (det3(src, 0, 2, 3) == 0.0) ||    
  37.             (det3(src, 1, 2, 3) == 0.0) ||    
  38.         (det3(dst, 0, 1, 2) == 0.0) ||    
  39.         (det3(dst, 0, 1, 3) == 0.0) ||    
  40.         (det3(dst, 0, 2, 3) == 0.0) ||    
  41.         (det3(dst, 1, 2, 3) == 0.0)) {
  42.     printf("3 points in line\n");
  43.     return;
  44.     }
  45.     idmap(dst, dsttrans);
  46.     idmap(src, srctrans);
  47.     invertmat3(srctrans, inv);
  48.     matmult3(inv, dsttrans, mat);
  49. }
  50.  
  51. mat3tomat4(mat3,mat4)
  52. float mat3[3][3];
  53. float mat4[4][4];
  54. {
  55.     int i, j;
  56.  
  57.     for (i = 0; i < 4; i++) 
  58.     for (j = 0; j < 4; j++)
  59.         mat4[i][j] = 0.0;
  60.     mat4[0][0] = mat3[0][0];
  61.     mat4[0][1] = mat3[0][1];
  62.     mat4[1][0] = mat3[1][0];
  63.     mat4[1][1] = mat3[1][1];
  64.  
  65.     mat4[0][3] = mat3[0][2];
  66.     mat4[1][3] = mat3[1][2];
  67.     mat4[3][3] = mat3[2][2];
  68.  
  69.     mat4[3][0] = mat3[2][0];
  70.     mat4[3][1] = mat3[2][1];
  71.  
  72.     mat4[2][2] = 0.0;
  73. }
  74.  
  75. identmat3(mat)
  76. float mat[3][3];
  77. {
  78.     mat[0][0] = 1.0;
  79.     mat[1][0] = 0.0;
  80.     mat[2][0] = 0.0;
  81.     mat[0][1] = 0.0;
  82.     mat[1][1] = 1.0;
  83.     mat[2][1] = 0.0;
  84.     mat[0][2] = 0.0;
  85.     mat[1][2] = 0.0;
  86.     mat[2][2] = 1.0;
  87. }
  88.  
  89. normmat3(mat) 
  90. float mat[3][3];
  91. {
  92.     float mag, del;
  93.  
  94.     del = mat[2][2]-1.0;
  95.     if(del>0.0001 || del<-0.0001) {
  96.     mag = mat[2][2];
  97.     mat[0][0] /= mag;
  98.     mat[1][0] /= mag;
  99.     mat[2][0] /= mag;
  100.     mat[0][1] /= mag;
  101.     mat[1][1] /= mag;
  102.     mat[2][1] /= mag;
  103.     mat[0][2] /= mag;
  104.     mat[1][2] /= mag;
  105.     mat[2][2] = 1.0;
  106.     }
  107. }
  108.  
  109. transpoint3(mat,x,y,tx,ty)
  110. float mat[3][3], x, y;
  111. float *tx, *ty;
  112. {
  113.     float div;
  114.  
  115.     div = (x*mat[0][2]+y*mat[1][2]+mat[2][2]);
  116.     *tx = (x*mat[0][0]+y*mat[1][0]+mat[2][0])/div;
  117.     *ty = (x*mat[0][1]+y*mat[1][1]+mat[2][1])/div;
  118. }
  119.  
  120. idmap(image, trans)
  121. float image[4][3], trans[3][3];
  122. {
  123.     int i, j;
  124.     float theta[3], D;
  125.  
  126.     D = det3(image, 0, 1, 2);
  127.     theta[0] = det3(image, 3, 1, 2)/D;    
  128.     theta[1] = det3(image, 0, 3, 2)/D;    
  129.     theta[2] = det3(image, 0, 1, 3)/D;    
  130.  
  131.     for (i = 0; i < 3; i++)
  132.     for (j = 0; j < 3; j++)
  133.         trans[i][j] = image[i][j]*theta[i];
  134. }
  135.  
  136. copymat3(smat,dmat)
  137. float smat[3][3];
  138. float dmat[3][3];
  139. {
  140.     bcopy(smat,dmat,9*sizeof(float));
  141. }
  142.  
  143. float det3(arr, i1, i2, i3)
  144. float arr[][3];
  145. int i1, i2, i3;
  146. {
  147.     float det;
  148.     
  149.     det = arr[i1][0]*(arr[i2][1]*arr[i3][2] - arr[i3][1]*arr[i2][2])
  150.      -arr[i1][1]*(arr[i2][0]*arr[i3][2] - arr[i3][0]*arr[i2][2])
  151.      +arr[i1][2]*(arr[i2][0]*arr[i3][1] - arr[i3][0]*arr[i2][1]);
  152.     return det;
  153. }
  154.  
  155. printmat3(trans)
  156. float trans[3][3];
  157. {
  158.     int i, j;
  159.  
  160.     for (i = 0; i < 3; i++) {
  161.     for (j = 0; j < 3; j++)
  162.         printf("%f\t", trans[i][j]);
  163.     printf("\n");
  164.     }
  165.     printf("\n");
  166. }
  167.  
  168. matmult3(x1, x2, prod)
  169. float x1[3][3], x2[3][3], prod[3][3];
  170. {
  171.     int i, j, k;
  172.  
  173.     for (i = 0; i < 3; i++)
  174.     for (j = 0; j < 3; j++) {
  175.         prod[i][j] = 0.0;
  176.         for (k = 0; k < 3; k++)
  177.         prod[i][j] += x1[i][k]*x2[k][j];
  178.     }
  179. }
  180.  
  181. #define det2(a, b, c, d) ((a)*(d)-(b)*(c))
  182.  
  183. invertmat3(mat, matinv)
  184. float mat[3][3], matinv[3][3];
  185. {
  186.     float a, b, c, d, e, f, g, h, i;
  187.     float a1, a2, a3;
  188.     float det;
  189.  
  190.     a = mat[0][0]; b = mat[0][1]; c = mat[0][2];
  191.     d = mat[1][0]; e = mat[1][1]; f = mat[1][2];
  192.     g = mat[2][0]; h = mat[2][1]; i = mat[2][2];
  193.  
  194.     det = a*det2(e, f, h, i)
  195.      -b*det2(d, f, g, i)
  196.      +c*det2(d, e, g, h);
  197.  
  198.     matinv[0][0] = det2(e, f, h, i)/det;
  199.     matinv[1][0] = -det2(d, f, g, i)/det;
  200.     matinv[2][0] = det2(d, e, g, h)/det;
  201.     matinv[0][1] = -det2(b, c, h, i)/det;
  202.     matinv[1][1] = det2(a, c, g, i)/det;
  203.     matinv[2][1] = -det2(a, b, g, h)/det;
  204.     matinv[0][2] = det2(b, c, e, f)/det;
  205.     matinv[1][2] = -det2(a, c, d, f)/det;
  206.     matinv[2][2] = det2(a, b, d, e)/det;
  207. }
  208.